//test.main
/** @file testMain.c
*   This file contains the full project with most kernel specific functions.
*   The purpose with this newer version is too keep one version always working correctly
*   @version 2 
*   @author Johan Svanborg */

#include "kernel.h"
#include "genericLinkedList.h"
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>

//Global var
TCB *Running;
list *readylist;
list * timerlist;
list * waitinglist;
mailbox * mailb;
uint ticks;

//Declaration
void cleanUp();
listobj *findListObjAllLists(TCB *);

#ifdef _MSC_VER
#else
/* IRQ handler */
__irq __arm void IRQ_Handler(void)
{
  /* We use only system timer interrupts, so we do not need
  to check the interrupt source. */
  int a = 2;
}
#endif

/** This is an idle task when nothing is happening */
void task_idle(void)
{
  //removeTCB(readylist, Running);
  while(1)
  {
    contextSwitch(); //This should be done in interupt irl
  }
}

void cleanUp()
{
  removeList(readylist);
  removeList(waitinglist);
  removeList(timerlist);
  remove_mailbox(mailb);
}

/** exception init_kernel()
*	This function initializes the kernel and its data
*	structures and leaves the kernel in start-up mode. The
*	init_kernel call must be made before any other call is
*	made to the kernel.
*	@return parameter  
*	@todo Set tick counter to zero */
exception init_kernel()
{
    //Create 3 linked lists
    timerlist = (list *)newList();
    readylist = (list *)newList();
    waitinglist = (list *)newList();
    mailb = create_mailbox(0,20);
    
    if(timerlist == NULL || readylist == NULL || waitinglist == NULL || mailb == NULL)
      return FAIL;
    
    return OK;
}

int order;

/** 
This function creates a task. If the call is made in startup
mode, i.e. the kernel is not running, only the
necessary data structures will be created. However, if
the call is made in running mode, it will lead to a
rescheduling and possibly a context switch.
Argument
@param *task_body A pointer to the C function holding the code of the task.
@param deadline The kernel will try to schedule the task so it will meet this deadline.
@return Description of the functions status, i.e. FAIL/OK. */
exception create_task(void(*task_body)(), uint deadline)
{
  TCB *task = NULL;
  task = (TCB *)newTCBObj(task_body, 0, deadline);
  if(task == NULL)
  {
    return FAIL;
  }
  
  if(Running == NULL)	//Startup mode
  {
    insertR(readylist, task);
  }
  else
  {
    ///@todo disable interupts
    SaveContext();
    if(TRUE)//if first execution then
    {
      //set not first execution
      //insert new task in readyllist
      insertR(readylist, task);
      //LoadContext();
    }
  }
  
  return OK;
}

/** 
This function starts the kernel and thus the system of
created tasks. Since the call will start the kernel it will
leave control to the task with tightest deadline.
Therefore, it must be placed last in the application
initialization code. After this call the system will be in
running mode. */
void run()
{
  //Initialize interupt timer here
  
  //Add idle task
  //create_task(task_idle, 20000);
  //Last
  Running = readylist->pHead->pNext->pTask;
  LoadContext();
}

/** 
This call will terminate the running task. All data
structures for the task will be removed. Thereafter,
another task will be scheduled for execution. */
void terminate()
{
  removeTCB(readylist,Running);
  
  contextSwitch();
  
  if(readylist->pHead->pNext == readylist->pTail) //Terminate kernel
  {
    cleanUp();
    while(1);
  }
}


/** Creates a new mailbox*/
mailbox* create_mailbox(int nof_msg, int size_of_msg)
{
    mailbox *mailb = NULL;
    mailb = (mailbox *) malloc(sizeof(mailbox)); //Allocate memory for the mailbox
    mailb->pHead = (msg *) malloc(sizeof(msg)); //Allocate memory for the head
    mailb->pTail = (msg *) malloc(sizeof(msg)); //Allocate memory for the head
    if(mailb == NULL || mailb->pHead == NULL || mailb->pTail == NULL)
            printf("Memory failure");
    mailb->pHead->pNext = mailb->pTail;
    mailb->pTail->pPrevious = mailb ->pHead;
    mailb->nMessages = nof_msg;
    mailb->nDataSize = size_of_msg;
    return mailb;
}

#ifdef _MSC_VER
#else
/**
This call will remove the Mailbox if it is empty and return
OK. Otherwise no action is taken and the call will return
NOT_EMPTY. */
exception remove_mailbox(mailbox *mBox)
{
  msg *iterator;
  msg *save;
  int nTest;
  if(mBox->pHead->pNext != mBox->pTail)
  {
          return NOT_EMPTY;
  }
  
  nTest = 1;
  iterator = mBox->pHead->pNext;
  save = NULL;
  while(iterator != mBox->pTail)
  {
          save = mBox->pHead->pNext;
          mBox->pHead->pNext = mBox->pHead->pNext->pNext;
  
          free(save->pBlock);
          free(save->pData);
          free(save);
          iterator = iterator->pNext;
  }
  
  free(mBox->pHead);
  free(mBox->pTail);
  free(mBox);
  
  return OK;
}
#endif
/**
Checks the mailbox for another task waiting for the msg.
RECIVER_SENDER determands if the msg is sent from a SENDER of a RECIVER.
*/
exception checkMailbox(mailbox* mBox, void* Data, int RECIVER_SENDER)
{
  //Check if recive_wait is waiting for msg
  msg *message = mBox->pHead->pNext;
  bool deadlineReached;
  uint count;
  while(message != mBox->pTail)
  {
          if(message->Status == RECIVER_SENDER)
          {
            if(ExistsInList(waitinglist, message->pBlock))
            {
              if(RECIVER_SENDER == RECEIVER)
              {
                message->pData = Data;
              }
              else
              {
                //* (int *)Data = * (int *)(message->pData);
                memcpy(Data, message->pData, sizeof(message->pData));
              }
                
              //Check deadline
              deadlineReached = message->pBlock->pTask->DeadLine < ticks;
              //Move reciver back to ready list.
              moveAndRemove(waitinglist, readylist, 1, message->pBlock);
              //Delete the msg if the running task is a receiver
              if(RECIVER_SENDER == SENDER)
                removeMessage(mBox, message);
              
              if(deadlineReached)
                return DEADLINE_REACHED;
              else
                return OK;
            }
          }
          message = message->pNext;
  }
  
  return FAIL;
}

/**
This call will send a Message to the specified Mailbox.
If there is a receiving task waiting for a Message on the
specified Mailbox, send_wait will deliver it and the
receiving task will be moved to the Readylist.
Otherwise, if there is not a receiving task waiting for a
Message on the specified Mailbox, the sending task will
be blocked. In both cases (blocked or not blocked) a
new task schedule is done and possibly a context
switch. During the blocking period of the task its
deadline might be reached. At that point in time the
blocked task will be resumed with the exception:
DEADLINE_REACHED. Note: send_wait  and
send_no_wait Messages shall not be mixed  in  the
same Mailbox. */
exception send_wait(mailbox* mBox, void* Data)
{
    msg *nMessage;
    exception e = checkMailbox(mBox, Data, RECEIVER);
    
    if(e == OK || e ==DEADLINE_REACHED)
    {
      SaveContext();
    }
    else
    {
      //Create msg
      nMessage = (msg *)newMsg();
      nMessage->pData = Data;
      insertMessage(mBox, nMessage, SENDER, readylist->pHead->pNext);
      
      //Block this sender
      moveAndRemove(readylist, waitinglist, 1, Running->pLObj);
      SaveContext();
    }
    
    if(!Running->IsResumed)
      {
        Running->IsResumed = 1;
        contextSwitch();
      }
      else
      {
        Running->IsResumed = 0;
        if(Running->DeadLine < ticks)
        {
          removeMessage(mBox, nMessage);
          return DEADLINE_REACHED;
        }
        
        return e;
      }
}

/** 
This call will send a Message to the specified Mailbox.
The sending task will continue execution after the call.
When the Mailbox is full, the oldest Message will be
overwritten. The send_no_wait call will imply a new
scheduling and possibly a context switch. Note:
send_wait and  send_no_wait Messages  shall not be
mixed in the same Mailbox. */
exception send_no_wait( mailbox* mBox, void* Data)
{
    msg *nMessage;
    
    exception e = checkMailbox(mBox, Data, RECEIVER);
    
    if(e != OK)
    {
      nMessage = (msg *)newMsg();
      nMessage->pData = Data;
      insertMessage(mBox, nMessage, SENDER, findListObjAllLists(Running)); //readylist->pHead->pNext
    }
    
    return e;
}

/** 
This call will attempt to receive a Message from the
specified Mailbox. If there is a send_wait or a
send_no_wait Message waiting for a receive_wait or a
receive_no_wait Message on the specified Mailbox,
receive_wait will collect it. If the Message was of
send_wait type the sending task will be moved to the
Readylist. Otherwise, if there is not a send Message (of
either type) waiting on the specified Mailbox, the
receiving task will be blocked. In both cases (blocked or
not blocked) a new task schedule is done and possibly
a context switch. During the blocking period of the task
its deadline might be reached. At that point in time the
blocked task will be resumed with the exception:
DEADLINE_REACHED.*/
exception receive_wait( mailbox* mBox, void* Data)
{
    msg *nMessage = NULL;
    
    exception e = checkMailbox(mBox, Data, SENDER);
    
    if(e == DEADLINE_REACHED)
    {
      return DEADLINE_REACHED;
    }
    else if(e != OK)
    {
        nMessage = (msg *)newMsg();
        nMessage->pData = Data;
        insertMessage(mBox, nMessage, RECEIVER, findListObjAllLists(Running));

        //Block reciving
        moveAndRemove(readylist, waitinglist, 1, findListObjAllLists(Running));
    }
    
    SaveContext();
    if(!Running->IsResumed)
    {
      Running->IsResumed = 1;
      contextSwitch();
    }
    else
    {
      //Returning from blocked state, either by deadline or sending task.
      Running->IsResumed = 0;
      //This task was blocked before and a msg should be waiting.
      if(e!=OK)
      {
        memcpy(Data, nMessage->pData, sizeof(nMessage->pData));
        removeMessage(mBox, nMessage);
      }
    }    
    
    return e;
}


/** 
This call will attempt to receive a Message from the
specified Mailbox. The calling task will continue
execution after the call. When there is no send
Message available, or if the Mailbox is empty, the
function will return FAIL. Otherwise, OK is returned.
The call might imply a new scheduling and possibly a
context switch. */
exception receive_no_wait( mailbox* mBox, void* Data)
{
  if(checkMailbox(mBox, Data, SENDER))
  {
    return OK;
  }
  else
  {
    msg *nMessage = (msg *)newMsg();
    nMessage->pData = Data;
    insertMessage(mBox, nMessage, RECEIVER, readylist->pHead->pNext);
  }
  return OK;
}

/** 
This call will block the calling task until the given
number of ticks has expired. */
exception wait(uint nTicks)
{
  readylist->pHead->pNext->nTCnt = ticks + 1;
  SaveContext();
  if(!Running->IsResumed)
  {
          Running->IsResumed = TRUE;
          //Set timer interupt here
          moveAndRemove(readylist, timerlist, -1, readylist->pHead->pNext);
          contextSwitch();
  }
  else
  {
    Running->IsResumed = FALSE;
  }
}

/** Checks the timerlist if any task should be moved to readylist.
    Checks waitinglist for task that has reached the deadline
    Finally loads context if that is needed. */
exception contextSwitch()
{
  TCB *task;
  bool taskFound = FALSE;
  
  //start with checking timerlist
  listobj *iterator = timerlist->pHead->pNext;
  ticks++;
  while(iterator != timerlist->pTail)
  {
    if(iterator->nTCnt < ticks)
    {
            task = iterator->pTask;
            moveAndRemove(timerlist, readylist, 1, iterator);
            taskFound = TRUE;
            break;
    }
    iterator = iterator->pNext;
  }
  if(!taskFound && readylist->pHead->pNext != readylist->pTail && Running != readylist->pHead->pNext->pTask)
  {
    task = readylist->pHead->pNext->pTask;
    taskFound = TRUE;
  }
  
  //Check blocked tasks for overridden deadlines
  iterator = waitinglist->pHead->pNext;
  while(iterator != waitinglist->pTail)
  {
    if(iterator->pTask->DeadLine < ticks)
    {
      task = iterator->pTask;
      moveAndRemove(waitinglist, readylist, 1, iterator);
      taskFound = TRUE;
      break;
    }
    iterator = iterator->pNext;
  }
  
  //If task is found then load context.
  if(taskFound)
  {
    Running = task;
    LoadContext();
  }
}

/** Gets the number of messages in the mailbox. */
int no_messages( mailbox* mBox )
{
  int nr = 0;
  msg *i = mBox->pHead->pNext;
  while(i != mBox->pTail)
  {
    nr++;
    i = i->pNext;
  }
  
  return nr;
}

/** 
This call will set the tick counter to the given value. */
void set_ticks( uint nTicks)
{
  ticks = nTicks;
}

/** 
This call will return the current value of the tick counter */
uint getTicks(void)
{
  return ticks;
}

/** 
This call will return the deadline of the running task */
uint deadline(void)
{
  return Running->DeadLine;
}

/** 
This call will set the deadline for the calling task. The
task will be rescheduled and a context switch might
occur. */
void set_deadline(uint deadline)
{
  ///@todo disable interupts
  SaveContext();
  /**IF first execution THEN
  Set: ?not first execution any more?
  Set the deadline field in the calling TCB.
  Reschedule Readylist
  Load context
  ENDIF*/
  Running->DeadLine = deadline;
  
  if(Running->IsResumed)
  {
    Running->IsResumed = FALSE;
  }
  else
  {
    
    Running->IsResumed = TRUE;
    moveAndRemove(readylist, readylist, 1, readylist->pHead->pNext);
    contextSwitch();
  }
}

listobj *findListObjAllLists(TCB *tcb)
{
  listobj *retItem;
  retItem = (listobj *)findListObj(readylist, tcb);
  if(retItem != NULL)
    return retItem;
  
  retItem = (listobj *)findListObj(waitinglist, tcb);
  if(retItem != NULL)
    return retItem;
  
  retItem = (listobj *)findListObj(timerlist, tcb);
  if(retItem != NULL)
    return retItem;
  
  return NULL;
}

/** 
This function is not available for the user to call.
It is called by an ISR (Interrupt Service Routine)
invoked every tick. Note, context is automatically saved
prior to call and automatically loaded on function exit. */
void TimerInt(void)
{
  /*Increment tick counter
Check the Timerlist for tasks that are ready for
execution, move these to Readylist
Check the Waitinglist for tasks that have expired
deadlines, move these to Readylist and clean up
their Mailbox entry.*/
  ticks++;
  contextSwitch();
}

/** 
This function is hardware dependent. All relevant
registers are saved to the TCB of the currently running
task. */
void SaveContext (void)
#ifdef _MSC_VER //if we are using visual studio.
{}
#else
;
#endif

/** 
This function is hardware dependent. All relevant
registers are restored from the TCB of the currently
running task to the CPU registers. */
void LoadContext (void)
#ifdef _MSC_VER
{}
#else
;
#endif

/** 
This function is not available for the user to call.
It is an ISR (Interrupt Service Routine) invoked every
tick. Note. It calls the C-Function TimerInt(). */
void timer0_isr (void);